#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <stdio.h>
#include <string>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>

#define CALIBCAMERA QString("Camera Only")
#define CALIBCAMERAVELODYNE QString("Camera -> Velodyne")
#define CALIBCAMERA2DLIDAR QString("Camera -> 2D LIDAR")

MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    loadConfig();

    std::vector<std::string> image_raw_topics;
    FILE *fp;

    if(!(fp = popen("rostopic list -v", "r"))) {
      fprintf(stderr, "cannot get image_raw topic list\n");
      exit(EXIT_FAILURE);
    }

    char topic[256];
    while((fgets(topic, 256, fp)) != NULL)
    {
     //if ((strstr(topic, "image_raw")) != NULL)
     std::string msg = std::string(topic);

     if (msg.find("Subscribed topics:") != std::string::npos)
         break;

        if (msg.find_first_of(']') == std::string::npos)
             continue;
        if (msg.find("Image") == std::string::npos)
            continue;

        msg = msg.substr(3, msg.find_first_of(']') - 2);

        image_raw_topics.push_back(msg);
    }

    if(image_raw_topics.empty())
    {
      QMessageBox msgBox(this);
      msgBox.setWindowTitle(tr("Warning"));
      msgBox.setIcon(QMessageBox::Warning);
      msgBox.setText(tr("No image_raw topic found.\nProgram exits."));
      msgBox.exec();
      exit(EXIT_FAILURE);
        image_raw_topics.emplace_back("/cam_lvds/front_left/image_raw/compressed");
    }

    pclose(fp);

    QStringList items;
    for (const auto& ch : image_raw_topics)
    {
      items << QString::fromStdString(ch);
    }

    QString inputcameratopic=QInputDialog::getItem(this, "Select Input", "Select Input Image Topic", items);
    QString selection=QInputDialog::getItem(this,"Choose Calibration Type", "Calibration Type",QStringList()<<CALIBCAMERAVELODYNE<<CALIBCAMERA2DLIDAR<<CALIBCAMERA,0,0);

    QSettings settings("RobotSDK","CalibrationToolkit");

    QSizeF patternsize=settings.value("PatternSize",QSizeF(0.035,0.035)).toSizeF();
    ui->patternwidth->setText(QString("%1").arg(patternsize.width()));
    ui->patternheight->setText(QString("%1").arg(patternsize.height()));
    cv::Size2f cvpatternsize=cv::Size2f(patternsize.width(),patternsize.height());

    QSize patternnum=settings.value("PatternNum",QSize(8,6)).toSize();
    ui->patterncolumn->setText(QString("%1").arg(patternnum.width()));
    ui->patternrow->setText(QString("%1").arg(patternnum.height()));
    cv::Size cvpatternnum=cv::Size(patternnum.width(),patternnum.height());

    std::cout << "start calibrate." << std::endl;

    if(selection==CALIBCAMERA)
    {
        QString cameratopic=inputcameratopic;
        CalibrateCameraChessboardROS * calibration=new CalibrateCameraChessboardROS(cameratopic,1000,10,cvpatternsize,cvpatternnum);
        ui->tabWidget->addTab(calibration,CALIBCAMERA);
        connect(ui->grab,SIGNAL(clicked()),calibration,SLOT(grabCalibDataSlot()));
        connect(ui->remove,SIGNAL(clicked()),calibration,SLOT(removeCalibDataSlot()));
        connect(ui->calibrate,SIGNAL(clicked()),calibration,SLOT(calibrateSensorSlot()));
        connect(ui->load,SIGNAL(clicked()),calibration,SLOT(loadCalibResultSlot()));
        connect(ui->save,SIGNAL(clicked()),calibration,SLOT(saveCalibResultSlot()));
        connect(ui->refresh,SIGNAL(clicked()),calibration,SLOT(refreshParametersSlot()));
        ui->Project->setEnabled(0);
    }
    else if(selection==CALIBCAMERAVELODYNE)
    {
        std::cout << "camera and lidar calibrate." << std::endl;

        std::vector<std::string> topAtyp = getTopicAndType(inputcameratopic);
        if (topAtyp.size() != 2){
            printf("please check topic and type.\n");
            return ;
        }

        QString cameratopic = QString::fromStdString(topAtyp[0]);
//        std::cout << topAtyp[0] << std::endl;
//        std::cout << topAtyp[1] << std::endl;

        QString velodynetopic="/rslidar_points";
        calibration=new CalibrateCameraVelodyneChessboardROS(
                cameratopic, topAtyp[1],1000,10,velodynetopic,1000,10,100,cvpatternsize,cvpatternnum);
//        calibration=new CalibrateCameraVelodyneChessboardROS(cameratopic,1000,10,velodynetopic,1000,10,100,cvpatternsize,cvpatternnum);


//        CalibrateCameraVelodyneChessboardROS * calibration=new CalibrateCameraVelodyneChessboardROS(cameratopic,1000,10,velodynetopic,1000,10,100,cvpatternsize,cvpatternnum);
        ui->tabWidget->addTab(calibration,CALIBCAMERAVELODYNE);
//        connect(ui->grab,SIGNAL(clicked()),calibration,SLOT(grabCalibDataSlot()));
        connect(ui->grab,SIGNAL(clicked()),this,SLOT(findchess()));

//        connect(ui->calibrate,SIGNAL(clicked()),calibration,SLOT(calibrateSensorSlot()));
        connect(ui->calibrate,SIGNAL(clicked()),this,SLOT(cali()));


        connect(ui->remove,SIGNAL(clicked()),calibration,SLOT(removeCalibDataSlot()));

        connect(ui->load,SIGNAL(clicked()),calibration,SLOT(loadCalibResultSlot()));
        connect(ui->save,SIGNAL(clicked()),calibration,SLOT(saveCalibResultSlot()));
        connect(ui->refresh,SIGNAL(clicked()),calibration,SLOT(refreshParametersSlot()));

//        connect(ui->Project,SIGNAL(clicked()),calibration,SLOT(projectPointsSlot()));
        connect(ui->Project,SIGNAL(clicked()),calibration,SLOT(projectPointsSlot_all_djw()));
    }
    else if(selection==CALIBCAMERA2DLIDAR)
    {
        QString cameratopic=inputcameratopic;
        QString lidartopic="/scan";
        CalibrateCameraLidarChessboardROS * calibration=new CalibrateCameraLidarChessboardROS(cameratopic,1000,10,lidartopic,1000,10,100,cvpatternsize,cvpatternnum);
        ui->tabWidget->addTab(calibration,CALIBCAMERA2DLIDAR);
        connect(ui->grab,SIGNAL(clicked()),calibration,SLOT(grabCalibDataSlot()));
        connect(ui->remove,SIGNAL(clicked()),calibration,SLOT(removeCalibDataSlot()));
        connect(ui->calibrate,SIGNAL(clicked()),calibration,SLOT(calibrateSensorSlot()));
        connect(ui->load,SIGNAL(clicked()),calibration,SLOT(loadCalibResultSlot()));
        connect(ui->save,SIGNAL(clicked()),calibration,SLOT(saveCalibResultSlot()));
        connect(ui->refresh,SIGNAL(clicked()),calibration,SLOT(refreshParametersSlot()));
        connect(ui->Project,SIGNAL(clicked()),calibration,SLOT(projectPointsSlot()));
    }
}

MainWindow::~MainWindow()
{
    QSettings settings("RobotSDK","CalibrationToolkit");
    settings.setValue("PatternSize",QSizeF(ui->patternwidth->text().toFloat(),ui->patternheight->text().toFloat()));
    settings.setValue("PatternNum",QSize(ui->patterncolumn->text().toInt(),ui->patternrow->text().toInt()));
    delete ui;
}

void MainWindow::loadConfig() {

//    std::string configPath = getDirectory() + "../../../config/config.yaml";
    std::string configPath = "/home/dawei/software/calibrate_tools/calibration_camera_lidar/config/config.yaml";

    cv::FileStorage read_(configPath,cv::FileStorage::READ);
    if (!read_.isOpened()){
        printf("[MainWindow::loadConfig]: %s open failed!\n",configPath.c_str());
        exit(-1);
    }
    read_["camPara"] >> cameraParaFile_;
    read_["findChessPath"] >> findChessPath_;
    read_.release();

    std::cout << "camParaFile: " << cameraParaFile_ << std::endl;
    std::cout << "findChessPath: " << findChessPath_ << std::endl;
}

void MainWindow::cali() {
    calibration->calibrateSensorSlot(cameraParaFile_);
}

void MainWindow::findchess() {
    calibration->grabCalibDataSlot(findChessPath_);
}

std::string MainWindow::getDirectory()
{
    char abs_path[1024];
    int cnt = readlink("/proc/self/exe", abs_path, 1024);//获取可执行程序的绝对路径
    if(cnt < 0|| cnt >= 1024)
    {
        return NULL;
    }

    //最后一个'/' 后面是可执行程序名，去掉devel/lib/m100/exe，只保留前面部分路径
    for(int i = cnt; i >= 0; --i)
    {
        if(abs_path[i]=='/')
        {
            abs_path[i + 1]='\0';
            break;
        }
    }

    std::string path(abs_path);

    return path;
}

std::vector<std::string> MainWindow::getTopicAndType(const QString &temps) {

    std::vector<std::string> topAtype;

    topAtype.clear();

    std::string megs = temps.toStdString();

    if (megs[0] != '/')
        return topAtype;

    int num = megs.find_first_of('[');
    int num_2 = megs.find_first_of(']');
    if (num == std::string::npos || num_2 == std::string::npos)
        return topAtype;

    topAtype.emplace_back(megs.substr(0, num - 1));
    topAtype.emplace_back(megs.substr(num + 1, num_2 - num -1));
    return topAtype;
}
